function [LogLikelihood,alpha,eta,a0tilde,outStru] = ...
    GeneralizedKFilterSmoother(y,Z,d,H,T,c,R,Q,Harvey)
%% GeneralizedKFilterSmoother.m
% GeneralizedKFilterSmoother.m is a version of the Kalman filter that
% augments the state when necessary to accomodate mixed frequency data
% and missing observations via the Harvey (1989) accumulator.
%  *Input:*
%    $y$ - matrix of the observables
%    $Z$ - structure for Z system matrix in the measurement equation
%    $H$ - structure for H system matrix of the measurement equation
%    $d$ - structure for d system vector of the measurement equation
%    $T$ - structure for T system matrix of the state equation
%    $c$ - structure for c system vector of the state equation
%    $R$ - structure for R system matrix of the state equation
%    $Q$ - structure for Q system matrix of the state equation
%    Harvey - structure to build accumulators into the state space model
%    Harvey.xi - vector of linear indices of mixed frequency series
%    Harvey.psi - matrix of full calendar of accumulator for each mixed
%                 frequency series
%  *Output:*
%    LogLikelihood    - loglikelihood value of the model
%    $\alpha$         - smoothed state of the model
%    $\tilde{a}_{0}$  - smoothed inference of initial condition
%
%   *More structure on Input Matrices* 
%
%   T=number of observations 
%   N=number of series 
%
%   Take general matrix $X$, if there are breaks, then must have 
%   $X.Xt$ which is 3 dimensional and $X.tauX$ which is [T+1 1] 
%   vector indicating which page to assign to each point in the sample 
%   plus, the T+1 observation, corresponding to the 1 step-ahead prediction
%   at the end of the sample
%   If there are no breaks, pass as a single 2 dimensional object, no need
%   to define additional subfields. 
% 
%   *More structure on Harvey* 
%    This is for mixed frequency estimation 
% 
%    1. Run harvey_accumulator.m to obtain indices for sums and
%    averages in mixed frequency 
% 
%    2. Define a vector [N 1] with accumulation types. 
%       0 = None 
%       1 = 

% 
%    Copyright: Scott Brave and R. Andrew Butters August 2012
%
% 
%  Changes by A. Justiniano October 2013 
%  1) Use SVD once to obtain all inversions for the filter and smoother 
%  2) Change initialization to the solution of the Discrete Lyapunov
%  Equation, if system is stationary; standard initialization crashes
%  system 
%  3) Allow H matrix to be empty, in which case simplify matrix
%  computations 
%  4) 


%% Diffuse Prior paramters
% $\kappa$ - diffuse prior parameter; see documentation
kappa = 10;
%% Calculate standard size parameters
% $p$ - number of observable series, $n$ - time series length
[p,n] = size(y);
%% Obtain system matrices from structure
% By passing both the different types of system matrices and the calendar
% vector corresponding to the particular type used at any particular time
% we allow for an arbitrary number of structural "breaks" in the state 
% space model. Furthermore, by allowing each system matrix/vector to have 
% its own calendar, no redundant matrices are saved in the workspace.

% Z system matrix
if isstruct(Z)
    tauZ  =Z.tauZ;
    Zt    =Z.Zt;
else
    tauZ = ones(1,n);
    Zt   = Z;
end
% d system matrix
if isstruct(d)
    taud = d.taud;
    dt    = d.dt;
else
    taud = ones(1,n);
    dt   = d;
end
% H system matrix
if isstruct(H)
    tauH = H.tauH;
    Ht    = H.Ht;
else
    tauH = ones(1,n);
    Ht   = H;
end
% T system matrix
if isstruct(T)
    tauT = T.tauT;
    Tt    = T.Tt;
else
    tauT = ones(1,n+1);
    Tt   = T;
end
% c system matrix
if isstruct(c)
    tauc = c.tauc;
    ct    = c.ct;
else
    tauc = ones(1,n+1);
    ct   = c;
end
% R system matrix
if isstruct(R)
    tauR = R.tauR;
    Rt    = R.Rt;
else
    tauR = ones(1,n+1);
    Rt   = R;
end
% Q system matrix
if isstruct(Q)
    tauQ = Q.tauQ;
    Qt    = Q.Qt;
else
    tauQ = ones(1,n+1);
    Qt   = Q;
end
%% Calculate transition equation size parameters
% $m$ - number of state variables, $g$ - number of shocks
[m,g] = size(Rt(:,:,end));
%% Augment Standard State Space with Harvey Accumulator
% $\xi$  - $\xi_{h}$ indice vector of each series accumulated
% $A$    - selection matrix
% $\psi$ - full history of accumulator indicator variable (for each series)
% $\tau$ - number of unique accumulator types
%        - Two Cases:'flow'    = 1 Standard Flow variable;
%                    'average' = 0 Time-averaged stock variable
if ~isempty(Harvey)
    xi = Harvey.xi;
    psi = Harvey.psi;
    
    type = any((psi==0),2);
    [s,r] = find((any((Zt(xi,:,:)~=0),3))');
    nonZeroZpos = zeros(length(xi),m);
    
    % Indentify unique elements of the state that need accumulation
    [APsi,~,nonZeroZpos(sub2ind([length(xi) m],r,s))] ...
        = unique([s type(r) psi(r,:)],'rows');
    
    tau = size(APsi,1);
    type = APsi(:,2);
    
    Ttypes   = [tauT' APsi(:,3:end)'];
    ctypes   = [tauc' APsi(:,3:end)'];
    Rtypes   = [tauR' APsi(:,3:end)'];
    
    [uniqueTs,~, newtauT] = unique(Ttypes,'rows');
    [uniquecs,~, newtauc] = unique(ctypes,'rows');
    [uniqueRs,~, newtauR] = unique(Rtypes,'rows');
    
    NumT = size(uniqueTs,1);
    Numc = size(uniquecs,1);
    NumR = size(uniqueRs,1);
    
    newT = zeros(m+tau,m+tau,NumT);
    for jj=1:NumT
        newT(1:m,:,jj) = [Tt(:,:,uniqueTs(jj,1)) zeros(m,tau)];
        for  h =1:tau
            if type(h) == 1
                newT(m+h,[1:m m+h],jj) = ...
                    [Tt(APsi(h,1),:,uniqueTs(jj,1)) ...
                    uniqueTs(jj,1+h)];
            else
                newT(m+h,[1:m m+h],jj) =...
                    [(1/uniqueTs(jj,1+h))*...
                    Tt(APsi(h,1),:,uniqueTs(jj,1))...
                    (uniqueTs(jj,1+h)-1)/uniqueTs(jj,1+h)];
            end
        end
    end
    
    newc = zeros(m+tau,Numc);
    for jj=1:Numc
        newc(1:m,jj) = ct(:,uniquecs(jj,1));
        for  h =1:tau
            if type(h) == 1
                newc(m+h,jj) = ct(APsi(h,1),uniquecs(jj,1));
            else
                newc(m+h,jj) = (1/uniquecs(jj,1+h))*...
                    ct(APsi(h,1),uniquecs(jj,1));
            end
        end
    end
    
    newR = zeros(m+tau,g,NumR);
    for jj=1:NumR
        newR(1:m,1:g,jj) = Rt(:,:,uniqueRs(jj,1));
        for  h =1:tau
            if type(h) == 1
                newR(m+h,:,jj) = Rt(APsi(h,1),:,uniqueRs(jj,1));
            else
                newR(m+h,:,jj) = (1/uniqueRs(jj,1+h))*...
                    Rt(APsi(h,1),:,uniqueRs(jj,1));
            end
        end
    end
    
    newZ = zeros(p,m+tau,size(Zt,3));
    for jj=1:size(Zt,3)
        newZ(:,1:m,jj) = Zt(:,:,jj);
        newZ(xi,:,jj) = zeros(length(xi),m+tau);
        for h=1:length(xi)
            cols = find(Zt(xi(h),:,jj));
            newZ(xi(h),m+nonZeroZpos(h,cols),jj) = Zt(xi(h),cols,jj);
        end
    end
    
    m    = m+tau;
    Tt   = newT;
    tauT = newtauT;
    Rt   = newR;
    tauR = newtauR;
    Zt   = newZ;
    ct   = newc;
    tauc = newtauc;
end
%% Clear unnecessary variables from workspace
clearvars -except y Zt tauZ dt taud Ht tauH Tt tauT ct tauc Rt tauR Qt...
    tauQ n p m g kappa NumberAccumulators NonZeroZPositions AccPositions
%% Establish Stationary & Non-stationary componenets of state
% Separate Cases into (i) Stationary
%
%
% $$a_{0} = \left(I_{m}-T_{\tau_{1}^{T}}\right)^{-1}c_{\tau_{1}^{c}}$$
%
%
% $$P_{0} = \left(I_{m^2}- T_{\tau_{1}^{T}} \otimes T_{\tau_{1}^{T}}
% \right)^{-1}\mbox{vec}(R_{\tau_{1}^{R}}Q_{\tau_{1}^{Q}}
% R_{\tau_{1}^{R}}^{\prime})$$
%
% (ii) Non-stationary state variables
%
% $$a_{0} = 0_{m\times 1}$$
%
%
% $$P_{0} = \kappa I_{m}$$
%

if all(abs(eig(Tt(:,:,tauT(1))))<0.999)
    if all( ct==0 )==true
        a0=zeros(m,1);
    else
        a0 = inv(eye(m)-Tt(:,:,tauT(1)))*ct(:,tauc(1));
    end
    P0=lyapunov_symm( Tt(:,:,tauT(1)) ,...
        squeeze(Rt(:,:,tauR(1)))*squeeze(Qt(:,:,tauQ(1)))*squeeze(Rt(:,:,tauR(1)))' );
else
    a0 = zeros(m,1);
    P0 = kappa*eye(m); % Note preset kappa value in line 36 of this code.
end
%% Pre-allocate Storage Matrices
v = zeros(p,n);
Finv = zeros(p,p,n);
K = zeros(m,p,n);
L = zeros(m,m,n);
P = zeros(m,m,n+1);
a = zeros(m,n+1);
LogL = zeros(1,n);
r = zeros(m,n+1);
alpha = zeros(m,n);
eta   = zeros(g,n);

%% Initialize filter
%
% $$a_{1}  =  T_{\tau^{T}_{1}}a_{0}+c_{\tau^{c}_{1}}$$
%
%
% $$P_{1}  =  T_{\tau^{T}_{1}}P_{0}T_{\tau^{T}_{1}}^{\prime}+
% R_{\tau^{R}_{1}}Q_{\tau^{Q}_{1}}R_{\tau^{R}_{1}}^{\prime} $$
%

a(:,1) = Tt(:,:,tauT(1))*a0+ ct(:,tauc(1));
P(:,:,1) = Tt(:,:,tauT(1))*P0*Tt(:,:,tauT(1))'...
    +Rt(:,:,tauR(1))*Qt(:,:,tauQ(1))*Rt(:,:,tauR(1))';
%% Kalman Filter
%
% $$Z_{\tau^{Z}_{t}}^{\star}  =  W_{t}Z_{\tau^{Z}_{t}} $$
%
%
% $$H_{\tau^{H}_{t}}^{\star} = W_{t}H_{\tau^{H}_{t}}W_{t}^{\prime}$$
%
%
% $$ d_{\tau^{d}_{t}}^{\star} = W_{t}d_{\tau^{d}_{t}}$$
%
%
% $$v_{t}  =  y_{t} - Z^{\star}_{\tau^{Z}_{t}}a_{t} -
% d_{\tau^{d}_{t}}^{\star}$$
%
%
% $$F_{t}  = Z^{\star}_{\tau^{Z}_{t}}P_{t}Z^{\star\prime}_{\tau^{Z}_{t}}
% + H^{\star}_{\tau^{H}_{t}}$$
%
%
% $$K_{t} = T_{\tau^{T}_{t+1}}P_{t}Z^{\star\prime}_{\tau^{Z}_{t}}
% F_{t}^{-1}$$
%
%
% $$L_{t} = T_{\tau^{T}_{t+1}} - K_{t}Z^{\star}_{\tau^{Z}_{t}}$$
%
%
% $$a_{t+1} = T_{\tau^{T}_{t+1}}a_{t}+c_{\tau^{c}_{t+1}} + K_{t}v_{t}$$
%
%
% $$P_{t+1}  =  T_{\tau^{T}_{t+1}}P_{t}L_{t}^{\prime} +
% R_{\tau^{R}_{t+1}}Q_{\tau^{Q}_{t+1}}R_{\tau^{R}_{t+1}}^{\prime}$$
%
for ii = 1:n
    
    % Create W matrix for possible missing values
    W = eye(p);
    ind = ~isnan(y(:,ii));
    W = W((ind==1),:);
    
    v(ind,ii) = y(ind,ii) - (W*Zt(:,:,tauZ(ii)))*a(:,ii)-W*dt(:,taud(ii));
    if isempty(Ht)==false
        tempF=(W*Zt(:,:,tauZ(ii)))*P(:,:,ii)*(W*Zt(:,:,tauZ(ii)))' ...
            + W*Ht(:,:,tauH(ii))*W';
    else
        tempF=(W*Zt(:,:,tauZ(ii)))*P(:,:,ii)*(W*Zt(:,:,tauZ(ii)))';
    end        
    tempF=0.5*(tempF+tempF'); 
    
    
    [PSVD,DSDV,PSVDinv]=svd(tempF);
    
    %% 1.b Truncate small singular values
    tolZero=1e-12;
    firstZero=min(find(diag(DSDV) < tolZero));
    if isempty(firstZero)==false
        PSVD=PSVD(:,1:firstZero-1);
        PSVDinv=PSVDinv(:,1:firstZero-1);
        DSDV=DSDV(1:firstZero-1,1:firstZero-1);
    end
    Finv(ind,ind,ii)=PSVD*(DSDV\eye(length(DSDV)))*PSVDinv';
    logDetF=sum(log(diag(DSDV)));

    %comparemat( Finv(ind,ind,ii), inv(tempF) ); 
    
    LogL(:,ii) = -1/2*(logDetF +...
        v(ind,ii)'*(  Finv(ind,ind,ii) )*v(ind,ii));
    K(:,ind,ii) = Tt(:,:,tauT(ii+1))*P(:,:,ii)*...
        (W*Zt(:,:,tauZ(ii)))'*Finv(ind,ind,ii);
    L(:,:,ii) = Tt(:,:,tauT(ii+1)) - K(:,ind,ii)*(W*Zt(:,:,tauZ(ii)));
    a(:,ii+1) = Tt(:,:,tauT(ii+1))*a(:,ii)+...
        ct(:,tauc(ii+1))+K(:,ind,ii)*v(ind,ii);
    if ii==size(y,2) 
        disp('End Point'); 
    end 
        
    P(:,:,ii+1) = Tt(:,:,tauT(ii+1))*P(:,:,ii)*L(:,:,ii)'+ ...
        Rt(:,:,tauR(ii+1))*Qt(:,:,tauQ(ii+1))*Rt(:,:,tauR(ii+1))';
 
end
%% If more than 4 output arguments, pass outStru as an output
if nargout > 4 
    outStru.P=P; 
    outStru.K=K; 
    outStru.L=L; 
    outStru.a=a;
    outStru.Finv=Finv; 
    outStru.v=v; 
else 
    outStru=[]; 
end 
    
%% Calculate Loglikelihood
%
% $$\mathcal{L} = - \frac{np}{2}\log(2\pi) -
% \frac{1}{2}(\log(|F_{t}|)+v_{t}^{\prime}F_{t}^{-1}v_{t})$$
%
LogLikelihood = -(sum(sum(isfinite(y)))/2)*log(2*pi) + sum(LogL);
%% Kalman Smoother
%
% $$r_{t} = Z_{\tau^{Z}_{t}}^{\star\prime}F_{t}^{-1}v_{t}+
% L_{t}^{\prime}r_{t+1}$$
%
%
% $$\hat{\alpha}_{t} = a_{t} + P_{t}r_{t}$$
%
%
% $$r_{n+1}=0$$
%
%
% $$\hat{a}_{0} = a_{0} + P_{0}T_{\tau^{T}_{1}}^{\prime}r_{1}$$
%

for ii = n:-1:1
    % Create W matrix for possible missing values
    W = eye(p);
    ind = ~isnan(y(:,ii));
    W = W((ind==1),:);
    
    r(:,ii) = (W*Zt(:,:,tauZ(ii)))'*Finv(ind,ind,ii)*v(ind,ii) + ...
        L(:,:,ii)'*r(:,ii+1);
    alpha(:,ii) = a(:,ii) + P(:,:,ii)*r(:,ii);
    eta(:,ii)    = Qt(:,:,tauQ(ii))*Rt(:,:,tauR(ii))'*r(:,ii);
end
a0tilde = a0 + P0*Tt(:,:,tauT(1))'*r(:,1);
%% End of File
end